/**
 * @file loam_registration.hpp
 * @author ysh
 * @brief LOAM 匹配模块
 * @version 0.1
 * @date 2023-05-10
 * 
 * @copyright Copyright (c) 2023
 * 
 */

#ifndef MODELS_REGISTRATION_LOAM_LOAM_REGISTRATION_HPP_
#define MODELS_REGISTRATION_LOAM_LOAM_REGISTRATION_HPP_

#include "../registration_interface.hpp"
#include "lidarFactorAuto.hpp"
#include "lidarFactorAnaly.hpp"
#include <pcl/kdtree/kdtree_flann.h>
#include <vector>
using namespace std;


class LOAMRegistration: public RegistrationInterface {
  public:
    LOAMRegistration();
    //LOAMRegistration(float res, float step_size, float trans_eps, int max_iter);

    bool SetInputTarget(const CloudData::CLOUD_PTR& input_target) override;
    bool ScanMatch(const CloudData::CLOUD_PTR& input_source, 
                   const Eigen::Matrix4f& predict_pose, 
                   CloudData::CLOUD_PTR& result_cloud_ptr,
                   Eigen::Matrix4f& result_pose) override;
 
  private:
  bool ExtractFullFeaturePoints(const CloudData::CLOUD_PTR& input_source,
                                  CloudData::CLOUD& cornerPointsSharp,
                                  CloudData::CLOUD& cornerPointsLessSharp,
                                  CloudData::CLOUD& surfPointsFlat,
                                  CloudData::CLOUD& surfPointsLessFlat);
  bool ExtractFeaturePoints(const CloudData::CLOUD_PTR& input_source,
                                  CloudData::CLOUD& cornerPointsSharp,
                                  CloudData::CLOUD& surfPointsFlat);
  bool FindCorresondenceForCornerFeatures(CloudData::CLOUD& current_corner_Points);
  bool FindCorresondenceForPlaneFeatures(CloudData::CLOUD& current_surf_Points); 
  bool IsSignificant(
    const Eigen::Matrix4f &transformation,
    const float trans_eps
);                             
  private:
    pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeCornerLast;
    pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeSurfLast;
    CloudData::CLOUD_PTR input_target_; 
    CloudData::CLOUD_PTR input_source_;
    CloudData::CLOUD_PTR last_cornerPointsSharp,
                      last_cornerPointsLessSharp,
                      last_surfPointsFlat,
                      last_surfPointsLessFlat;
    vector<Eigen::Vector3d> curr_cornerpoints,last_cornerpoints_a,last_cornerpoints_b;
    vector<Eigen::Vector3d> curr_surfpoints,last_surfpoints_a,last_surfpoints_b,last_surfpoints_c;
    Eigen::Matrix4f transformation_;
};
#endif